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Abstract —The problem of optimal feedback planning among 
obstacles in (i-dimensional configuration spaces is considered. We 
present a sampling-based, asymptotically optimal feedback planning 
method. Our method combines an incremental construction of 
the Delaunay triangulation, volumetric collision-detection module, 
and a modified Fast Marching Method to compute a converging 
sequence of feedback functions. The convergence and asymptotic 
runtime are proven theoretically and Investigated during numerical 
experiments, in which the proposed method is compared with the 
state-of-the-art asymptotically optimal path planners. The results 
show that our method is competitive with the previous algorithms. 
5-^ Unlike the shortest trajectory computed by many path planning 
O-ialgorithms, the resulting feedback functions can be used directly for 
robot navigation in our case. Finally, we present a straightforward 
extension of our method that handles dynamic environments where 
obstacles can appear, disappear, or move. 
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I. INTRODUCTION 

We present the Asymptotically-optimal Control over Incre¬ 
mental Delaunay simplicial Complexes (ACIDIC) method for 
computing an optimal feedback control among obstacles in d- 
dimensional configuration spaces. In this method, a sample 
sequence, the Delaunay triangulation, and a volumetric collision- 
detection module are used to build a simplicial free space approx¬ 
imation. This volumetric approximation parallels the Probabilistic 
RoadMap (PRM) [16] in that it captures the topology of the free 
space and has the same asymptotic computational complexity as 
its optimal implementation PRM* [15]. Contrary to the PRM 
and PRM*, using the Fast Marching Method (FMM) [28] on this 
simplicial approximation enables planning for a near-optimal path 
through the volume of d-dimensional cells instead of constraining 
it to edges of an ID graph; see Fig. [T] Moreover, borrowing 
ideas from graph search literature [19], [20], [31], we extend 
the FMM to handle vertex insertions and deletions, which led 
us to efficient asymptotically optimal feedback planning and 
replanning algorithms. 

For almost three decades, computing the optimal robot motion 
has been a central problem in robotics. Optimal solutions are 
required when resources such as fuel and time are expensive or 
limited. However, the optimal planning problem is computation¬ 
ally hard. For example, a relatively “easy” problem of finding 
the shortest path in a 3-dimensional polygonal environment is 
already PSPACE-hard [5]. Thus, only approximate optimal paths 
are computed in practice. 
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Fig. 1: Incremental feedback plan (color) and resulting path 
(white) created by the proposed method for a robot (lower left) 
that desires to reach a goal (upper right) while avoiding obstacles 
(black). Regions for which a policy is still unknown are gray. 
Movies, including replanning in dynamic environments, appear 
at http : //tinyurl. com/q jnazvr 


Recently, there has been a flurry of interest in sampling- 
based methods that provide asymptotic optimality by improving 
iteratively near-optimal solutions. For example, RRG* [15] con¬ 
structs a rapidly-exploring random graph (RRG) using a sample 
sequence of random vertices. In practice, such algorithms can be 
terminated after a sufficiently accurate solution has been found. 
In this respect, they are similar to anytime graph-search algo¬ 
rithms [20]. Contrary to graph-search algorithms, the planners 
such as RRG converge to the true optimal path in the limit, as 
the iteration number tends to infinity. 

Most sampling-based planning algorithms are path-centric, that 
is, they find an open-loop control that defines a near-optimal 
trajectory from start to goal [1], [10], [24], [25]. When used 
for robot navigation, such algorithms require an additional path¬ 
following controller to close the control loop. On the other 
hand, control-centric algorithms compute a feedback function that 
stabilizes the dynamical system towards the goal—eliminating the 
need of auxiliary controllers. Nonoptimal feedback planning al¬ 
gorithms are well established in robotics [21], [27], [32]. The first 
incremental, sampling-based, asymptotically optimal feedback 
stochastic control planner is presented in [13]. Introduced in [34], 
Simplicial Dijkstra and A* algorithms compute approximate opti¬ 
mal feedback control using a simplicial free space decomposition 
and the FMM, which recently have been combined with the 
adaptive mesh refinement yielding an asymptotically optimal, but 
not sampling-based feedback planning algorithm [33]. 

Our algorithm is different from the previous attempts to 
combine sampling strategies with the FMM. In [12], for example, 
the RRT algorithm is used to compute the initial path, which 
is improved during the post-processing using the FMM over a 













local Delaunay triangulation. Since only a local approximation 
is used, the convergence to the globally optimal path is not 
guaranteed. Despite a naming similarity, the Fast Marching Trees 
(FMT*) algorithm [14] does not implement the FMM. The FMT* 
computes cost-to-go wavefront propagation using its values from 
the neighboring vertices instead of interpolating between them. 
Thus, it is closer in spirit to Dijkstra’s graph search algorithm. 

To the best of our knowledge, the ACIDIC is the first asymptot¬ 
ically optimal feedback planning method that combines sampling 
strategies, volumetric free space approximations, and efficient 
FMM-based feedback planning algorithms. 

II. PRELIMINARIES 
A. Optimal Feedback Planning Problem 

An optimal control formulation provides the most natural 
framework for the feedback planning algorithm. Let X be the 
configuration space of a robot, that is, the complete set of 
parameters and their respective ranges that uniquely determine 
robot’s internal configuration and position in the world. The 
open obstacle set, Xobs C X, corresponds to all states of 
the robot that result in either self-collisions or collisions with 
obstacles in the world. The robot is free to traverse the free space, 
Xfree = \ -^obs, along trajectories of an ordinary differential 

equation (ODE) with control 

x{t) = f{x{t),u{f)) . (1) 

This equation defines the dynamics of the robot and allows the 
input signal u{t), chosen from the input set U, to control it. 
Einally, we assume that Xf^ee is a compact Lipschitz domain, 
that is, the boundary of X^ee can be represented locally as a 
level-set of a Lipschitz continuous function. 

In many problems in robotics the configuration space X and 
the motion model Q are well-defined due to careful mechanism 
design. The obstacle set, on the other hand, inherits the complex¬ 
ity of the surrounding world, which is less predictable and may 
also evolve over time. 

The performance of the robot is measured with respect to an 
additive cost functional; 

J{x,u)= f c{x{t),u(t)) dt (2) 

Jo 

if x{t) G Xfree for all t G and J(x,u) = oo otherwise. 

In the above, u and x are realizations of the input signal and the 
corresponding trajectory, c : X x 17 —>■ K+ is a positive local 
cost, and t{ is the terminal time. Usually, tf is the first moment 
when x{t{) is inside the goal set Xgoai. We assume, Xgoai is a 
compact Lipschitz domain in X^ee- The task of optimal motion 
planning algorithm is to minimize J. 

We employ Bellman’s dynamic programming principle and 
find the optimal control using the cost-to-go function V. At all 
points X G X, V{x) is defined as the optimal cost of reaching 
the goal from x, and it satisfies Hamilton-Jacobi-Bellman partial 
differential equation (HJB PDE): 

min{VU(x) •/(a;, u)-f c(a;, u)} = 0 . (3) 

uGU 


Once the cost-to-go is computed, the feedback control 
is given as a minimizing argument in (|^, 7r(x) = 

argmin„gy{VU(x) • f{x,u) 7- c(x, u)}. The optimal trajectory 
is then found by replacing u with 7r(x) in ([T]i and integrating the 
resulting ODE on the interval 

B. HJB Numerical Discretization 

When an analytical solution is unavailable for (|^, we must 
resort to numerical PDE solvers. To this end, we follow closely 
the EMM discretization [28]. 

Definition 1 (Sample Set): A finite or countable set of distinct 
vertices X is called a sample set of X. 

Definition 2 (Abstract and Geometric Simplices): An 
abstract simplex of dimension c? is a set of (d + 1) vertices 
T = {xo,..., Xd} C X such that Xk Xk' for all k and k'. A 
geometric realization of r is a convex hull of its vertices, which 
we call geometric simplex and denote X^.- Formally, 

d d 

Wak > 0 such that E afc = l}. (4) 

k=0 k=0 

In the above, are called barycentric coordinates of a 

point X = J2k=o^kXk G Xr- Finally, Xr' is called a (proper) 
facet of Xt if t' is a (proper) subset of t. 

Definition 3 (Vertex Set Triangulation): A triangulation of X 
is a set of abstract d-dimensional simplices T such that for all 
t,t' gT and t t' the intersection Xr fj Xr' is a proper facet 

of both simplices and the union UtgT covers the convex hull 

of X. 

The numerical solution is computed in three steps. 

First, we consider a triangulation of a sample set of vertices in 
X. An approximation of X^ee is then derived from this vertex 
set triangulation by ignoring simplices that have a nonempty 
intersection with Xobs- Note that this approximation approaches 
Xfree in the limit, as the dispersion of the vertex set tends to 
zero. 

Second, using a collision-free triangulation of X^ee, we define 
a piecewise linear cost-to-go approximation; 

y{x)= X! y{xk)ak{x ), (5) 

XkGr 

in which t is an abstract simplex such that x G Xr, and ak{x) 
is fcth barycentric coordinate of x in Xr - Note that V is defined 
completely using its values at sampled vertices. 

Finally, we substitute © into © and derive the discrete 
Hamilton-Jacobi-Bellman equation at vertex x G X: 

min min {XrV(x) ■ f (x, u)c(x, u)} = 0 . (6) 

TGSt(ai) uGUx,t 

In the above, XrV is a restriction of XV onto simplex 
Xr and St(x) = {r | x G t} is called a star of vertex x 
and represents a local neighborhood around x. The minimiza¬ 
tion over input signal u is constrained to the set Ux,r = 


X, = 


{S 





{m € t7 I 3(5 > 0 : X + 6f{x,u) G Xr}. This constraint is nec¬ 
essary to construct a positive coefficient discretization, which 
converges to the viscosity solution of ([^ [9]. 

The linear convergence rate of the numerical discretization (|^ 
has been established in [34], that is, E < Ch, in which E = 
sup 2 ,gjf \V{x) — 14(x)| is the global numerical error, and h = 
sup^g-y-max 2 ,^a;/gT- ||x — x'\\ is the maximum edge length. In the 
above, C > 0 is a constant, which is independent of h. 

When considered at all vertices x G X, equation defines 
a system of nonlinear equations. Similar to Dijkstra’s algorithm, 
the Fast Marching Methods (FMM) [28] evaluates the cost-to- 
go values in a single pass trough the vertex set using a priority 
queue. If the triangulation is acute, then the FMM solves this 
nonlinear system [18], [29]. 

C. Delaunay Triangulation 

Definition 4 (Delaunay Triangulation): The Delaunay (also 
known as Delone) triangulation, T, of a sample set X is such 
that, for all T S T and all x € X \ r, x is located outside of the 
circumsphere of Xt- 

General vertex set triangulations are difficult to compute, with 
the notable exception of the Delaunay triangulation (DT). In com¬ 
putational geometry, the bijection between Delaunay simplices in 
and lower faces of the convex hull of sample vertices lifted 
onto a paraboloid in has been established. Thus, geometric 
convex hull algorithms can be used for constructing the DT. It 
also follows from this relation that the DT exists and is unique if 
sample vertices are in general position (that is, there are no (i-|-2 
vertices such that they belong to some d-dimensional sphere). 

Moreover, compared with other vertex set triangulations, the 
DT maximizes the min-containment radius of simplices [26] 
and minimizes the second-order error of a piecewise linear 
interpolation in 2D [3] and in d-dimensional case [6]. The former 
implies that the DT is the most regular of all triangulations, and 
the latter guarantees the smallest possible numerical error in (|^. 

Considering the simplicity of computing the DT and its op¬ 
timality with respect to interpolation error, we use the DT for 
approximating the cost-to-go function on Xf^ee- 

D. Stochastic Delaunay Triangulation 

Random sampling has been proven useful for high-dimensional 
path planning problems. Randomized motion planning algorithms 
have both theoretical and practical advantages. In theory, they are 
probabilistically complete, which guarantees finding an existent 
solution. In practice, they rapidly explore the environment search¬ 
ing for a solution. 

Following this trend, we present the Delaunay triangulation of 
a random vertex set, which we call stochastic Delaunay trian¬ 
gulation (SDT). From the motion planning perspective, the SDT 
can be compared with the PRM in that they both approximate the 
free space, as the number of sample vertices increases. Unlike the 
PRM graph, however, the SDT is a volumetric approximation of 
Xfree- Although computing simplex collisions is generally more 
difficult than performing an 1-dimensional collision check, this 


volumetric information enables finding paths that traverse through 
simplicial cells instead of being constrained on graph edges. 

In this section, we establish a connection between the SDT 
and Poisson-Delaunay mosaics and some useful properties of the 
former that follow from the integral geometry. 

Definition 5 (Poisson Point Process in A Poisson point 
process in of intensity p is a set of random points such that for 
two bounded, measurable, disjoint sets Xi and X 2 , the number 
of points of this process that are inside Xi and X 2 are two 
independent Poisson random variables with parameter pffiXfi) 
and pp,{X 2 ), respectively^ 

Definition 6 (Poisson-Delaunay Mosaic): Let X he a realiza¬ 
tion of a Poisson process. The Delaunay triangulation of X is a 
Poisson-Delaunay mosaic]^ 

It follows from Definition that a set of N uniformly 
and independently distributed vertices from the free space is 
the restriction of the Poisson point process. Using maximum 
likelihood estimate, we establish the intensity of this process; 
p = N/p,{Xfree)- This relation shows that the SDT can be 
considered as the restriction of a Poisson-Delaunay mosaic. 

In integral geometry, statistical properties of Poisson-Delaunay 
mosaics have been established. For example, the expected number 
of simplices in the star of a vertex depends on the dimension 
number d, but it is independent of the intensity p [17]. Therefore, 
the expected number of vertex-adjacent simplices is independent 
of N for the SDT^ 

Another property of Poisson-Delaunay mosaics is related to the 
average simplex size as a function of the point process intensity. 
The average edge length is [23], and the expected 

maximum edge length is 0((log[4]. Using previously 
derived relation between p and N, we find that the expected 
edge length and its maximum of the SDT are and 

0((log A^/Af)^/'^), respectively!^ 

III. SAMPLING-BASED FEEDBACK PLANNING 
ALGORITHM 

We now present the ACIDIC method for sampling-based 
feedback planning. The execution trace of our method is similar 
to most sampling-based path-centric planners: 

1) Sample a new vertex Xnew from X; 

2) Refine the Delaunay triangulation to include Xnew; 

3) Update the cost-to-go values and associated feedback con¬ 
trol in the simplicial approximation of Xfree- 

At a conceptual level, the ACIDIC method “etches” the free 
space away from the obstacle set while simultaneously refining a 
feedback control. In the limit of infinitely many sampled vertices, 
all points of Xf^ee become part of the triangulation and the 
optimal feedback control is computed. 

'Here, /r(X) denotes the Lebesgue measure of X. 

^The definition of DT can be extended to locally sparse infinite sets. 

^Note that our optimal planner has a constant branching factor in expectation 
compared with 0{log At) branching factor for RRT*. 

■^Note that exactly the same bound on the shrinking radius enables percolation 
limit in the RRG-based algorithms. 


Algorithm 1 ACIDIC method (planning version) 
Input: Compact Lipschitz domains Agoai C Afree C X; 

bounding simplex tq such that Afree C Xtq 
Output: Yields tt 
1: global V, DT, tq, QP 

2: Let C(ro) ^0, r ^ {ro}, and DT ^ (r,C, St) 

3: Initialize V{x) ^ oo for all a: € To 

4: loop 

5: Xnew SamplePoint() 

6: InsertPoint(xnew) 

7: InitiateFMM({a:new}) 

8: CompleteFMM() 

9: yield 7r(a:) = argmin{Vt7(a:) • f{x,u) + c{x,u)} 

u€U 


Algorithm 2 ACIDIC method 
(replanning version) 

Input: Compact Lipschitz domains Agoai Q ^free Q 
bounding simplex tq such that Afree C Xtq 
Output: Yields tt 
1: global V, DT, tq, QP 

2: Let C(ro) ^0, r ■{- {to}, and DT ^ (T,C, St) 

3: Initialize V{x) <r- oo for all a: € To 

4: loop 

5: if Moving Robot then 

6: Move Robot 

7: Xne'N SamplePoint(); Aset {a^new} 

8: InsertPoint(a:new) 

9: if Obstacle Changes Observed then 

10: A,,t ^ Aset u (U (^ ! n AAobs ^ 0}) 

11: InitiateFMM(Aset) 

12: CompleteFMM() 

13: yield ■k{x) = arg min{ VP (a:) • /(a:, u) + c(a:, u)} 

u€U 


* Note that we disallow cost-to-go values at two dif¬ 
ferent vertices to depend on each other. Thus, in 
VertexLookAhead(), it is assumed V(x') = oo at 
all x' ^ T such that V{x') — minloc(x', r, y) when 
computing y*(a:). 


Algorithm 3 InsertPoint(x) 

1: 77is ComputeVisible(a:) 

2\ "R, <r- ComputeRidges(77is) 

3: for all p ^ 71 do 

4: T ^ pU{a:}; T ^ C(t) ^ 0 

5: for all r' G 77is do 

6: C('r')<-C(r')U{'r} 

7: for all x £ p do 

8: St(a:')^ (St(a:') Ui'^l) \ 

9: St(a:) ^ St(a:) 


Algorithm 4 ComputeVisible(x) 

1: Initialize LIFO queue Q and Tv\s ^ 0 
2: T FindSimplex(x) 

3: Push(Q,T) 

4: while Q ^ 0 do 
5: Pop(Q, t) 

6: if IsVisible(a:, t) then 

7: 7;is^rvisUW 

8: for all t' G U{St(a:)}3;er \ 77is do 

9: Push(Q,T'} 

10: return Tvis 


Algorithm 5 ComputeRidges(7^is) 

1: 7^^0 

2: for all t G 77is do 
3: for all X G T do 

4: p ^ r \ {x} 

5: if ^ t' G 77is \ {t} : p C t then 

6: n^nuip} 

7: return TZ 


Algorithm 6 FindSimplex(x) 

1: T To 

2: while C(t) y 0 do 
3: for T G C{t) do 

4: if X G Xt' then 

5: T ^ T^ 

6: break for loop 

7: return r 


Algorithm 7 InitiateFMM(Aset) 

1: for all x G Aset do 
2: V"(x) <— oo 

3: Remove(QP, x) 

4: for all x G Aset do 
5: VertexLookAhead(x) 

6: UpdateQueue(QP, x) 


Algorithm 8 VertexLookAhead(x)* 

1: y*(x) <— oo 
2: for T G St(x) do 
3: if IsCollisionFree(T) then 

4: ^*(^) min{minloc(x, T, y), 

V'-(x)} 


Algorithm 9 CompleteFMM() 

1: while QP y 0 do 
2: X ^ Pop(QP) 

3: if y(x) > y*(a:) then 

4: y(x) y*(x) 

5: else if y(x) < y’^(x) then 

6: y(x) oo 

7: for all x' G U{'^}x€St{:c) \ do 

8: VertexLookAhead(x') 

9: UpdateQueue(QP, x') 

10: if y(x) = oo then 

11: VertexLook Ahead(x) 

12: UpdateQueue(QP, x) 


Algorithm 10 UpdateQueue(QP, x) 

1: if y*(x) < y(x) — e or y*{x) > y(a;) 

then 

2: if X ^ QP then 

3: Push(QP,x) 

4: A(x) <— min{y(x), y*(x)} 

5: else if x G QP then 
6: Remove(QP, x) 


This basic idea can be used to create an entire family of feed¬ 
back planning algorithms. The behavior of a particular algorithm 
depends on three factors: the sampling strategy, the volumetric 
decomposition (that also includes collision-detection strategy), 
and the methods by which the approximate cost-to-go function 
and feedback control are updated. 

We discuss our implementation of these three steps in Sec¬ 
tions III-A-III-C respectively. An illustrative example is pre¬ 


sented in Section |III-C.1| and an extension to replanning in 
dynamic environments in Section [riI-C.2 Our generic implemen¬ 
tation can be improved for specific applications. 


A. Sampling Strategy 

Motivated by theoretical results from Section we explored 
three different sampling strategies, which we implemented in 
Algorithm 1: uniform random sampling, deterministic largest- 


simplex Delaunay refinement, and goal-oriented refinement in the 
vicinity of the current shortest path and near obstacle boundaries. 

In theory, uniform random sampling guaranties regularity and 
convergence of the DT in expectation. In practice, however, 
biased or carefully engineered deterministic sequences are likely 
to improve planning algorithm performance by constructing high- 
quality triangulations. 

The largest circumradius of all Delaunay simplices defines 
the Euclidean dispersion of the sampled vertex set. Moreover, 
the circumcenter of the corresponding simplex maximizes the 
distance to the closest sample vertex. By placing Xnew at this 
circumcenter, we optimize the sample sequence to consider 
unexplored regions of the configuration space. This strategy is 
known as Delaunay refinement [7], and is proven to produce a 
high-quality triangulation [30]. 

In the limit, Delaunay refinement samples vertices uniformly 


















































in the configurations space. However, the convergence of our 
algorithm can be improved by sampling regions in which the 
interpolation error is the highest. We consider, for example, 
regions where the cost function is highly nonlinear, such as near 
obstacle boundaries or the goal set as well as the vicinity of 
the current optimal path. In particular, we choose Xnew as the 
circumcenter of the largest circumsphere of Delaunay simplices 
that are either on the boundary of Xobs and Xgoed or contain the 
current shortest path. 

B. Computing Volumetric Free Space Approximation 

At every iteration. Algorithm 3 uses a newly sampled vertex 
to improve the volumetric approximation of Xf^ee- To update 
the Delaunay triangulation, we follow closely the incremental 
convex hull algorithm introduced in [2]. To this end, we find 
all simplices that are visible from the newly inserted vertex; see 
Algorithm 4. Next, we find a set of ridges, which are defined as 
faces separating visible and invisible simplices; see Algorithm 5. 
A set of new Delaunay simplices is created by connecting all 
ridges with the inserted vertex; see Line 4 in Algorithm 3. Finally, 
we “remove” visible simplices by updating their children sets to 
include all newly inserted simplices. 

Note that all simplices are actually retained; however, only 
childless simplices belong to the current DT, which is reflected in 
updating the local connectivity information in St(a;) and St(a;'); 
see Lines 7-9 in Algorithm 3. The remaining simplices organized 
into a directed acyclic graph structure that helps locating future 
vertices within the Delaunay triangulation, as it is prescribed by 
Algorithm 6. 

After the DT is updated, a black box collision-detection 
module is used to find free-to-traverse simplices in the current 
triangulation. We assume the conservative collision-detection 
implementation, that is, the simplex is considered collision-free 

if n Xobs = 0. 

It is worth noting that pointwise collision-detection modules 
are ill-suited for volumetric representations because each simplex 
contains an infinite number of points to check. Thus, as it is also 
the case with many graph-based planning algorithms, we require 
additional information about the obstacle set to compute the 
volumetric free space approximation. For example, if the distance 
to the nearest obstacle is known and higher-order dynamics, 
such as velocities, are bounded, then we can verify collision-free 
simplices by solving a convex minimization problem. Volumetric 
collision-detection is an advanced topic, and further implemen¬ 
tation details are beyond the scope of the current paper. 

C. Computing Cost-To-Go Function and Feedback Control 

The cost-to-go function and the optimal feedback control are 
maintained using a modified version of the Fast Marching Method 
(FMM). In particular, our version addresses a nonmonotonic cost- 
to-go wavefront propagation, which may be caused by either a 
nonacute triangulation or a part of the free space approximation 
becoming the obstacle set due to local simplex rewiring and a 
conservative collision-detection. 


Our modifications to the FMM follow closely replanning path¬ 
centric strategies, such as D* Lite [19] and RRT^ [24], which 
deal with appearing and disappearing obstacles by propagating 
the increase wavefront ahead of the decrease wavefront. To 
prevent infinite loops, the algorithm interrupts wavefront prop¬ 
agation when changes are smaller than a given parameter e. At 
the end of this section, we investigate a peculiar side effect of 
our implementation: a straightforward extension to an optimal 
feedback replanning algorithm. 

1) Fast Marching Feedback Planning Algorithm: The ACIDIC 
planning algorithm (Algorithm 1) improves incrementally the 
feedback control by initiating and propagating the cost-to-go 
wavefront (Lines 7 and 8, respectively). 

The wavefront is initiated at newly sampled vertex x^ew, when 
the look-ahead value V* becomes inconsistent with the current 
cost-to-go value V; see Algorithm 7. The look-ahead value is 
computed in Algorithm 8 using minloc function that solves the 
inner minimization problem in (|^ for collision-free simplices. 
The implementation of minloc and its geometric interpretation 
are discussed in [34]. Finally, we also assume minloc returns 0 
if X is in the goal set. 

The inconsistency between V and V* implies that the solution 
of the nonlinear system is not found yet. Algorithm 9 repairs 
inconsistency by propagating the cost-to-go wavefront through 
the environment in the fast marching fashion. To this end, the 
vertices are organized in a priority queue in the increasing order 
of their key values K = minjC, (/*}. For each vertex x, two 
cases are considered: 1) cost-to-go value decreases (Line 3) 
and 2) cost-to-go value increases (Line 5). In the first case, 
V{x) is updated to its current best estimate, V*{x), and the 
decrease wavefront is propagated to vertex neighbors lowering 
their V values. In the second case, V{x) is set temporarily to 
infinity causing V* values to increase at all neighbors whose 
cost-to-go values are depended on V{x). Thus, the increase 
wavefront is propagated. Next, V*{x) is updated creating the 
decrease wavefront, which repairs inconsistencies introduced by 
the increase wavefront. 

When Algorithm 9 terminates, the cost-to-go function is con¬ 
sistent with up to a small e error in the entire domain. 
However, if the initial position is known the global cost-to-go 
computations are unnecessary, and the estimate of the cost-to- 
come can be used to restrict the computation domain. Various 
heuristics and their effect on computations are discussed in [8], 
[34]. 

2) Fast Marching Replanning Algorithm: The goal of the 
ACIDIC replanning algorithm is to update the feedback control 
as soon as robot’s sensors detect a change in obstacle configu¬ 
ration. Fast control loop relies crucially on replanning algorithm 
efficiency. In this case obstacles may appear or disappear, which 
results in increasing or decreasing cost-to-go values. Fortunately, 
ACIDIC planning algorithm accounts for both of these changes, 
and its extension towards replanning algorithm is rather straight¬ 
forward. 

We present Algorithm 2, in which wavefront propagation is 


initiated once the change in obstacle set is confirmed (Line 10). 
A volumetric collision-detection module is used to find all 
inconsistent vertices that are affected by obstacle changes. This 
conservative estimate guarantees that after wavefront propagation 
the computed cost-to-go function is consistent with ([^ up to a 
small e error. 


IV. ANALYSIS OF THE ALGORITHM 


A. Numerical Convergence 

The resolution of the sample set increases when each additional 
vertex is inserted. Hence, the accuracy of the planning algorithm 
is expected to improve. This intuition can be rigorously supported 
combining the results of Section [n| 

Theorem 7 (Numerical Convergence): We assume e = 0 and 
N random vertices are sampled. The expected solution error, 
E[£:], is then 0((^)3). 

Proof: The proof follows from the numerical error bound 
presented in Section [ILB and the expected maximum edge length 
presented in Section II-D ■ 
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Fig. 2: Temperature map of the cost-to-go values (color) and 
path (white) computed by ACIDIC algorithm during the first 10 
seconds in a randomly generated 2D environment of size 100 x 
100. Delaunay simplices are outlined in black, and obstacles are 
solid black. 


vertex is constant. Maintaining the priority queue, however, takes 
0{\ogN) time. ■ 

V. EXPERIMENTS, RESULTS, AND DISCUSSION 


B. Computational Complexity 


To establish the computational complexity of Algorithm 1, we 
consider its expected runtime per iteration. 

It should be noted that the worst-case runtime of our algorithm 
is bounded from below by the maximum number of Delaunay 
simplices in the triangulation of N vertices, which is proportional 
to [22]. However, such artificially constructed cases 

“rarely” occur in practice. Thus, algorithms that we proposed for 
vertex insertion and wavefront propagation are output-sensitive 
in that they have optimal complexity 0{\ogN) per Delaunay 
simplex. In conjunction with the results from Section II-D we 


prove the expected runtime bounds for the ACIDIC method. 

Lemma 8: Let T be a DT of a random vertex set X, and let 
a^new € X. Eor the closest vertex x* = argmin^^j^ ||a:new — x\\, 
there exists r G St (a:*) such that x is inside the circumsphere of 
X.. 


Proof: We omit the proof due to space limitations. ■ 
Theorem 9 (Delaunay Triangulation Complexity): The 
expected runtime of Algorithm 3 is O(logX) per iteration. 

Proof: Since the expected size of St (a;) is independent 
of N, the visible simplex number and the ridge number are 
constant in expectation. Thus, average simplex rewiring runtime 
is constant. Moreover, from Lemma it follows that finding the 
first visible simplex can be done in expected 0(log N) time using 
Kd-trees [11]. ■ 

Note that our algorithm for finding the first visible simplex 
avoids using Kd-trees for simplicity. In theory, it is not yet 
clear that the average depth of the children graph is OilogN). 
However, this algorithm performs well in practice. 

Theorem 10 (Wavefront Propagation Complexity): The 
expected runtime of Algorithm 9 is 0{logN) for all e > 0. 

Proof: Since the approximate cost-to-go function converges, 
the number of e-changes of V at each vertex is bounded. 
Thus, the number of times wavefront propagates through each 


The basic idea presented in this paper can be used to create 
an entire family of incremental volumetric feedback planning 
algorithms. For brevity, we limit our investigation to the imple¬ 
mentations proposed in Section III In Sections V-A and V-B we 


experimentally evaluate the performance of feedback planning in 
static environments and compare our results with state-of-the-art 
graph-based methods. In Section V-C we present simulations of 
the replanning version of the algorithm in a dynamic environment. 

All experiments were run in simulated environments on a Dell 
Optiplex 790 with Intel i7 chip and 16GB of RAM; a single 
processor core is used for all experiments. All algorithms are 
implemented in Julia programming language. Both volumetric 
and graph-based algorithms use the same code-base in order 
to make the comparison as fair as possible. For example, all 
algorithms use the same sampling schemes, obstacle representa¬ 
tions, collision-detection routines, heap data-structures, control- 
loop computations, data-logging procedures, and so on. 


A. Point Robot in Random 2D Static Environment 

In this experiment, a holonomic single integrator point-robot 
desires to travel to a goal point among randomly generated 
polygonal obstacles in 2D; see Fig. We compare our ACIDIC 
planning algorithm with RRT* and RRT^. Results of most basic 
versions appear in Fig. while a comparison of variants that use 
different sampling techniques and values of e appears in Fig. 

In theory, volumetric and graph-based methods have identical 
asymptotic performance per iteration. In experiments, however, 
we observe that volumetric methods have a constant factor 
overhead associated with updating the Delaunay triangulation 
and computing fast marching wavefront propagation. Thus, our 
method processes fewer vertices per unit of wall time. Despite 
the difference in sampling performance, convergence rates of 
all considered methods are comparable, which implies that the 
ACIDIC method is more efficient in using sampled vertices. We 
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Fig. 3: Averaged over 50 trials relative path error (left, log-log 
scale) and the vertex number (right, regular scale) against wall 
time for ACIDIC algorithm using Delaunay refinement with e = 0 
(purple) and e = 1 (orange), RRT* (black dot-dash), and RRT# 
(black solid) in the environment of Fig. 
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Fig. 5; Averaged over 50 trials relative path error (left, log-log 
scale) and the vertex number (right, log-log scale) against wall 
time for ACIDIC algorithm (solid) and RRT# (dashed) in 2D to 
5D environments (color). 
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Fig. 4: Averaged over 50 trials relative path error (left, log-log 
scale) and the vertex number (right, regular scale) against wall 
time in the environment of Fig. Results are for ACIDIC al¬ 
gorithm using Delaunay refinement (solid) and random sampling 
(dashed), e = 0 (dark/light purple) and e = 1 (orange/yellow), 
uniform (dark purple/orange) and focused (light purple/yellow) 
sampling, and RRT# (black). 


attribute this efficiency to allowing the path traverse through the 
volume of space potentially ignoring all sampled vertices. 

We optimized the convergence rate of the ACIDIC method 
using only the information gained from the volumetric decom¬ 
position. First, refining near obstacle boundaries is accomplished 
by sampling simplices that are partially in collision. This focused 
sampling enables the discovery of narrow corridors in the free 
space. Hence, the optimal solution can be found using fewer 
vertices. Second, the convergence rate is improved by refining the 
triangulation in the vicinity of the best path. Finally, truncating 
wavefront propagation reduces the runtime per iteration. The op¬ 
timized ACIDIC method outperforms all graph-based algorithms 
considered in our experiments. 

Note that truncating the wavefront propagation by setting e = 1 
significantly improved the convergence in static environments. In 
graph-based methods, the usefulness of such this idea appears 
to be limited to dynamic environments, in which fast wavefront 


propagation and the control-loop speed are primary concerns. We 
believe the reason why the truncation benefits the convergence of 
ACIDIC methods is due to the larger constant factor associated 
with recomputing the cost-to-go function through the triangula¬ 
tion. 

B. Block Obstacle in 

In Fig. 1^ we evaluate the performance of ACIDIC method 
in dimensions between 2 and 5. The environment used for each 
dimension is a hypercube with a single prismatic obstacle located 
at the center. The obstacle spans one-half of the environment 
in the first two dimensions and has infinite length along other 
dimensions. 

As with many planning methods, the volumetric idea suffers 
from the “curse of dimensionality”. From the experiments, it 
became evident that the runtime per vertex grows exponentially 
with the dimension number for the ACIDIC algorithm. Note 
that this overhead is constant in any particular dimension, and 
the expected vertex insertion complexity for ACIDIC algorithm 
remains 0{\ogN), in which N is the number of already inserted 
vertices. Opposite to the volumetric methods, the runtime per 
vertex is largely independent of the dimension number for graph- 
based algorithms, which, unfortunately, does not yield their faster 
convergence. From the convergence results, we confirm that 
volumetric methods use vertices more efficiently. We attribute this 
to the fact that d+1 vertices are sufficient to construct a simplex 
covering large volume of the free space. On the other hand, graph- 
based methods may require many more sample vertices to recover 
the shortest path. 

C. Replanning Simulations in Dynamic Environments 

Due to space limitations and also the fact that navigation 
through dynamic environment is best visualized in media that 
incorporates time, we have uploaded a playlist of movies to 
http : //tinyurl. com/q jnazvr that illustrate the dynamic 
version of ACIDIC replanning algorithm. 





























The replanning version of ACIDIC is the first asymptotically- 
optimal sampling-based feedback replanner that is capable of 
repairing the feedback control in real-time when obstacles un¬ 
expectedly appear, disappear, or move within the configuration 
space. While RRT^ is closely related to our algorithm, it is 
path-centric and requires a feedback control to be computed in 
post-processing. While this can be done quickly, in practice, the 
particular method that is used will significantly affect reaction 
time — the most important performance measure of a replanning 
algorithm. Thus, we do not compare the two methods directly. 

VI. SUMMARY 

Proposed in this paper, is the first asymptotically optimal feed¬ 
back planning algorithm that uses a volumetric approximation of 
free space in conjunction with the Fast Marching Method. The 
main idea of the method is to build an incremental Delaunay 
triangulation use a sample sequence and compute a feedback 
control in all collision-free simplices. The idea can be used for 
both planning in static environments and real-time replanning in 
dynamic environments. 

We have established theoretical convergence guarantees and 
asymptotic per iteration computational complexity. During the 
experiments in simulated environments, we confirmed theoretical 
results and compared the performance of our implementation with 
that of state-of-the-art asymptotically optimal planners. It was 
established that the performance of a our basic implementation 
is similar to that of the previous planning algorithms. However, 
optimizing sampling and wavefront propagation routines proved 
beneficial for a substantial performance increase. 

While being closely related to the previous asymptotically- 
optimal graph-based planners, such as, RRT*, RRT^, and RRT^, 
the ACIDIC method is fundamentally different from all of them. 
In particular, instead of computing an one-dimensional path on 
a graph, our method computes a collision-free feedback control 
that stabilizes the system towards the goal in the entire volume 
of the free space. Thus the output of the ACIDIC method can 
be used directly to control the system, and the implementation 
of auxiliary path-following controllers can be avoided. Analysis 
and experiments show that the ACIDIC method is theoretically 
sound and works well in practice. 
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